#include<ros/ros.h>
#include<sensor_msgs/Joy.h>
#include<mavros_msgs/ActuatorControl.h>

class TeleopUav
{
    public:
        TeleopUav();
    private:
        void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);

        ros::NodeHandle nh_joy;

        double roll_scale,pitch_scale,thrust_scale;
        ros::Publisher con_pub_;
        ros::Subscriber joy_sub_;

};

TeleopUav::TeleopUav()
    {
       /*
       nh_joy.param<double>("roll_scale",roll_scale,1);
       nh_joy.param<double>("pitch_scale",pitch_scale,1);
       nh_joy.param<double>("thrust_scale",thrust_scale,1);
       */

      roll_scale=1;
      pitch_scale=1;
      thrust_scale=1;

      con_pub_=nh_joy.advertise<mavros_msgs::ActuatorControl>("joy_initial",10);
      joy_sub_=nh_joy.subscribe<sensor_msgs::Joy>("joy",10,&TeleopUav::joyCallback, this);

    }

void TeleopUav::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{
    mavros_msgs::ActuatorControl input;
    input.controls[0]=roll_scale*joy->axes[0];//roll
    input.controls[1]=pitch_scale*joy->axes[1];//pitch
    input.controls[2]=0;//yaw
    input.controls[3]=thrust_scale*joy->axes[3];//thrust

    con_pub_.publish(input);
}

int main(int argc, char** argv)
{
    ros::init(argc,argv, "teleop_uav");
    TeleopUav teleop_uav;

    ros::spin();
}